/*
	motor.cpp
*/
#include "motor.h"
#include <stdio.h>


using namespace std;
//using namespace Comm;
//using namespace NXT;
//using namespace NXT::Sensor;


int HTPort;

void ShrMotor::waitMS (int ms) 
{
	clock_t endwait;
	endwait = clock() + ms * CLOCKS_PER_SEC/1000 ;
	//cout << "\t" << (double)clock()/CLOCKS_PER_SEC << endl;
	while (clock() < endwait) {}
	//cout << "\t" << (double)clock()/CLOCKS_PER_SEC << endl;
}
/* The setup() function tels the NXT brick to treat the device
connected to the given port as a motor controller. It also sets up
the motor controller to take commands and execute them, and tells
Motor 1 that it needs to rotate backwards because it is on the
right so that positive rotations roll the robot forward. */
void ShrMotor::setup(int port) 
{
	HTPort = port;
	// set motor port to I2C
	ViUInt8 directCommandBuffer[] = 
	{ SETINPUTMODE,
		HTPort,
		LOWSPEED9V,
		RAWMODE 
	};
	comm.SendDirectCommand( false,	reinterpret_cast< ViByte* >(directCommandBuffer ),sizeof( directCommandBuffer ), NULL, 0);
	// set power to 0, constant speed, reset encoder, reverse motor 1
	i2cWrite(M1_MODE, MODE_NTO | MODE_REV);
	i2cWrite(M2_MODE, MODE_NTO);
	resetPosition();
	setModeSpeed();
	setSpeed(0,0);
}
/* The resetPosition() function zeros out the encoder positions. This
function is very important because the encoder counts are limited
to 16bits, which is not enough with the sprocket ratios on the
robot to count a full 360 degree turn. This function should be
called before attempting to servo the robot. If it is not, the
robot will only turn the difference between te given angle and what
it believs its current angle is. Note that setting the mode in any
way for motor 1 resets MODE_REV for some unknown reason, so it
needs to be reminded it is running in reverse. */
void ShrMotor::resetPosition() 
{
	i2cWrite(M1_MODE,(i2cRead(M1_MODE) & MODE_MSK)| MODE_RST | MODE_NTO	| MODE_REV); // MODE_REV reminder
	i2cWrite(M2_MODE,(i2cRead(M2_MODE) & MODE_MSK)| MODE_RST| MODE_NTO);
}
/* The getPosition() function returns the current position of the
encoders in a position structure. This function used to be less
important, but will be more heavily used in this revision of the
code as it will be checked after telling the motors to move to
ensure that the command wasn't lost and the motors are actually
moving. The NXT and HiTechnic motor controller cannot be trusted
to always turn a command to an actuation, so they will need to be
monitored by the software to ensure they respond correctly. */
position ShrMotor::getPosition() 
{
	position ticks;
	ticks.left = i2cRead(M1_ENC,4);
	ticks.right = i2cRead(M2_ENC,4);
	return ticks;
}
/* The setModeSpeed() function puts the motor controller into constant
speed mode. Under constant speed mode, the encoders are used
solely to control the wheel speeds, and not used for positioning at
all. This mode will be used for travelling long distances over
long time frames where the laptop can montor the robot's position
using the sensors like the cameras and also possibly by monitoring
the encoders and resetin them as necessary. It can allow the robot
to travel great distances inaccurately. */
void ShrMotor::setModeSpeed() 
{
	i2cWrite(M1_MODE,(i2cRead(M1_MODE) & MODE_MSK)	| MODE_SPD	| MODE_NTO	| MODE_REV); // Remind motor 1 it runs in reverse.
	i2cWrite(M2_MODE,	(i2cRead(M2_MODE) & MODE_MSK)	| MODE_SPD	| MODE_NTO);
}
/* The setModeServo() function puts the motor controller into servoing
mode where the encoders are used for positioning instead of speed
control. It is much more accurate than constant speed mode but due
to the limits on the encoder counts cannot be used to travel great
distances or turn at large angles. */
void ShrMotor::setModeServo() 
{
	i2cWrite(M1_MODE,	(i2cRead(M1_MODE) & MODE_MSK)	| MODE_SRV	| MODE_NTO	| MODE_REV); // Motor 1 is still reversed.
	i2cWrite(M2_MODE,	(i2cRead(M2_MODE) & MODE_MSK)	| MODE_SRV	| MODE_NTO);
}


void ShrMotor::setSpeed(double spdLeft, double spdRight) 
{	
	position start, current;
	double slowestNonZeroSpeed;
	/* Calculate the speed cap factor if needed. */
	double capFactor = min(1.0,	TOP_SPEED / max(abs(spdLeft),abs(spdRight)));
	/* Adjust the speeds if needed. */
	spdLeft*= capFactor;
	spdRight*= capFactor;
	slowestNonZeroSpeed = min(abs(spdLeft), abs(spdRight));
	if (slowestNonZeroSpeed == 0)
		slowestNonZeroSpeed = max(abs(spdLeft), abs(spdRight));
	start = getPosition();
	/* Converts the speed to a power and get things moving (hopefully) */
	i2cWrite(M1_PWR, speed2pwr(spdLeft));
	i2cWrite(M2_PWR, speed2pwr(spdRight));
	
}

void ShrMotor::sleep(unsigned int mseconds)
{
    clock_t goal = mseconds + clock();
    while (goal > clock());
}

void ShrMotor::stop() 
{
	setTargetTicks(0,0);
	resetPosition();
	setSpeed(0,0);
}
/* The getTarget() function returns the current target in mm. It does
this by calling the getTargetTicks() function and then converting
from ticks to mm. This function will most likely only be called
during debugging. */
position ShrMotor::getTarget() 
{
	position ticks;
	ticks.left = i2cRead(M1_TRG,4)/TIC_PER_MM;
	ticks.right = i2cRead(M2_TRG,4)/TIC_PER_MM;
	return ticks;
}

/* The setTargetTicks() function sets the target position in ticks
relative to the current position of both motors. It then gets the
target position back from the motors to make sure the target
position was actually set correctly. Be warned that the encoder
range limitations mean that this function could cause an encoder
overflow if the encoder positions are already near the limits.
Therefore, unless it is being used for small corrections in
position, it should probably only be called after zeroing the
positions of both encoders. */

void ShrMotor::setTargetTicks(int trgLeft, int trgRight)
{
	position here = getPosition();
	int ticLeft, ticRight;
	int returnedTicLeft, returnedTicRight;
	int attempt;
	ticLeft = trgLeft + (int)here.left;
	ticRight = trgRight + (int)here.right;
	for (attempt = 0; attempt < MOTOR_COMMAND_RETRIES; attempt++) 
	{
		for (int i=0; i<4; i++) 
		{ /* Target is a 32bit
			value stored in 4
			sequential bytes. */
			i2cWrite(M1_TRG+i, ticLeft>>(8*(3*i))& 0xFF );
			i2cWrite(M2_TRG+i, ticRight>>(8*(3*i)) & 0xFF );
		}
		
		returnedTicLeft = i2cRead(M1_TRG,4);
		
		returnedTicRight = i2cRead(M2_TRG,4);
		
		if (returnedTicLeft == ticLeft && returnedTicRight == ticRight) 
			break;
	}
}

/* The setTarget() function sets the target position in mm of both
motors relative to the current position. It does this by calling
the setTargetTicks() function with values generated with dist2tic()
function. */

void ShrMotor::setTarget(double trgLeft, double trgRight)
{
	setTargetTicks(dist2tic(trgLeft),dist2tic(trgRight));
}

/* The getVoltage() function returns the current battery voltage in
volts. This function will let the laptop know when the robot is
getting hungry. */

double ShrMotor::getVoltage() 
{
	return (double) i2cRead(BATT_V) * .08;
}

/* The drive() function drives the robot forward at the given speed in
mm/s */

void ShrMotor::drive(double speed) 
{
	setModeSpeed();
	setSpeed(speed, speed);
}


/* The driveTo() function drives forward to the specified position
given in mm. Please be aware that the limits on the encoder range
places extreme limits on the range this function is able to target,
especially if the encoder positions are already close to their
maximum before the function is called. for maximum range and
accuracy out of this function, it's probably best to zero out the
encoder positions before calling it. */

void ShrMotor::driveTo(double dist, double speed)
{
	setModeServo();
	setTarget(dist, dist);
	setSpeed(speed, speed);
}

/* The turnAngle() function turns the robot clockwise to the specified
angle. In other words, a positive angle will turn the robot to the
right and a negative angle will turn the robot to the left. This
function used to be significantly more complex before the new
OmniWheel drive system was installed. Now it simply describes a
radius based on the width of the center wheelbase. */

void ShrMotor::turnAngle(double angle, double revolutionsPerSecond) 
{
	angle = angle;
	// + = right, =	left
	/* The circumference of the circle traveled by the center two
	conventional wheels. */

	double circumference = 2 * PI * MOTOR_W;
	/* The slipRatio is no longer needed for the power
	calculations because the corner wheels, being OmniWheels,
	can now slide sideways freely. This code has been left in,
	but commented out, for historical purposes and also in case
	the old wheel architecture is ever reinstalled.
	*/
	/* Ratio of power applied to the wheels on the [left,right]
	side to power doing useful work, i.e. not slipping
	sideways. */
	/*
	double slipRatio = fabs( sqrt( pow(MOTOR_W,2) + pow(MOTOR_L,2) )
	/ (MOTOR_W) );
	circumference *= slipRatio;
	*/
	/* The speed is determined by the circumference of the circle
	transcribed by the center two wheels and the revolutions
	per second. */

	double speed = circumference * revolutionsPerSecond;

	/* The target, likewise, is determined by the transcribed
	circumference and the intended angle. */

	double target = circumference * angle/360;
	setModeServo();
	setTarget(target,target);

	/* And GO. Note that there may be some drifting because the
	motors don't always move when told to. The setSpeed()
	function attempts to correct for this, but if the motors
	don't both fire up on the first go there will still be some
	offset in the movement as one motor gets a head start on
	the other. there is no way to fix that without fixing the
	phantom communication errors between the NXT and the
	hiTechnic motor controller. */

	setSpeed(speed, speed);

	//cout << "circ" << circumference << endl;
	//cout << "speed" << speed << endl;
}

/* The turn() function turns the robot counterclockwise the specified
speed. It does set a target angle or put the motors controller
into servoing mode, it just turns. */

void ShrMotor::turn(double direction, double revolutionsPerSecond) 
{
	direction = direction / abs(direction); // set to +1
	/* The circumference is just like in the turnAngle() function. */

	double circumference = 2 * PI * MOTOR_W;

	/* And just like the turnAngle() function, we don't need the
	slipRatio calculations any more, but we're still leaving them in,
	but commented out. */
	/* Ratio of power applied to the wheels on the [left,right] side to
	power doing useful work, i.e. not slipping sideways. */
	/*
	double slipRatio = fabs( sqrt( pow(MOTOR_W,2) + pow(MOTOR_L,2) )
	/ (MOTOR_W) );
	circumference *= slipRatio;
	*/
	/* Just like in turnAngle(). */

	double speed = circumference * revolutionsPerSecond;

	setModeSpeed();

	/* Again, there may be some drifting as one motor fails to pay
	attention and start spinning for some reason. */
	setSpeed(direction*	speed, direction*speed);
}

/* The busy() function USUALLY returns true if the motors are enroute
to the target position. BUT NOT ALWAYS. For example, the first
time it's called, it almost always returns a false negative. DO
NOT TRUST THIS FUNCTION. Generally, if the function returns a
true, it is, in fact, busy. However, sometimes when it says it
isn't busy, it actually is. It's probably a good idea to confirm
with the encoder positions whenever it this function claims it
isn't busy. */

int ShrMotor::busy() 
{
	return (i2cRead(M1_MODE) & MODE_BSY) || (i2cRead(M2_MODE) & MODE_BSY);
}
/* The i2cWrite() function tells te NXT to writes a byte to the
hiTechnic motor controller via the i2c connection. This code
apears to be functional and is directly copied form the original
motor.cpp code. */

void ShrMotor::i2cWrite(int address, int data) 
{
	// prevent buffer overruns
	waitMS(10);
	// write 3 bytes, expect 0 back, 0x02 is padding
	ViUInt8 directCommandBuffer[] =	{ LSWRITE,HTPort,0x03,0x00,0x02,address,	data	};
	ViUInt8 responseBuffer[] = { 1,1};
	comm.SendDirectCommand( true,	reinterpret_cast< ViByte* >( directCommandBuffer ),	
		sizeof( directCommandBuffer ),	reinterpret_cast< ViByte* >( responseBuffer ),
		sizeof( responseBuffer ));
}
/* The i2cRead() function reads four or less bytes from the given
address over the NXT's i2c connection to the hiTechnic motor
controller. This code has also been kept from the original
code. */

int ShrMotor::i2cRead(int address, int bytes) 
{
	// prevent buffer overruns
	waitMS(10);
	// ask for data, wait till it arrives
	int bytesRead = 0;
	do {
		// write three bytes, expect some back, 0x02 is padding
		ViUInt8 directCommandBuffer[] = {LSWRITE,HTPort,0x02,bytes,	0x02,address};
		ViUInt8 responseBuffer[] = {1, 1};
		comm.SendDirectCommand( true,
			reinterpret_cast< ViByte* >( directCommandBuffer ),
			sizeof( directCommandBuffer ),
			reinterpret_cast< ViByte* >( responseBuffer ),
			sizeof( responseBuffer));
		bytesRead = NXT::Sensor::LSGetStatus(&comm, HTPort);//(HTPort);
	} while (bytesRead < bytes);
	// read data
	ViUInt8 directCommandBuffer2[] = {LSREAD, HTPort};
	ViUInt8 responseBuffer2[] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};
	comm.SendDirectCommand( true,
	reinterpret_cast< ViByte* >( directCommandBuffer2 ),
	sizeof( directCommandBuffer2 ),
	reinterpret_cast< ViByte* >( responseBuffer2 ),
	sizeof( responseBuffer2 ));
	long signed int result=0;
	for (int i = 3; i<3+bytes; i++) 
	{
		result <<= 8;
		result |= responseBuffer2[i];
	}
	return result;
}

/* The speed2pwr() function takes a speed in mm/s and converts it to a
power level sent to the hiTechnic motor controllers using the
TIC_PER_SEC_PER_PWR constant defined in schrodie_motor.h that was
determined through trial and error experimentation. This function
could probably be replaced with a simple preprocessor
macro. */

int ShrMotor::speed2pwr(double speed) 
{
	return (int)(speed*TIC_PER_MM)/TIC_PER_SEC_PER_PWR;
// mm/s * tick/mm * (s/tick/pwr)
}
/* The dist2tic() function converts a distance in mm to encoder ticks
using the constant TIC_PER_MM defined in schrodie_motor.h. This
constant was also deterined experimentally. This function could
also be easily replaced with a preprocessor
macro. */
int ShrMotor::dist2tic(double mm) 
{
	return (int)(mm*TIC_PER_MM); // mm * tick/mm = tick
}